#include "LSM303A.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "rpi.h"




#define LSM303A_ADDRESS           (0x30 >> 1)

// register addresses
#define LSM303A_CTRL_REG1_A       0x20
#define LSM303A_CTRL_REG2_A       0x21
#define LSM303A_CTRL_REG3_A       0x22
#define LSM303A_CTRL_REG4_A       0x23
#define LSM303A_CTRL_REG5_A       0x24
#define LSM303A_HP_FILTER_RESET_A 0x25 
#define LSM303A_REFERENCE_A       0x26
#define LSM303A_STATUS_REG_A      0x27

#define LSM303A_OUT_X_L_A         0x28
#define LSM303A_OUT_X_H_A         0x29
#define LSM303A_OUT_Y_L_A         0x2A
#define LSM303A_OUT_Y_H_A         0x2B
#define LSM303A_OUT_Z_L_A         0x2C
#define LSM303A_OUT_Z_H_A         0x2D


#define LSM303A_INT1_CFG_A        0x30
#define LSM303A_INT1_SRC_A        0x31
#define LSM303A_INT1_THS_A        0x32
#define LSM303A_INT1_DURATION_A   0x33
#define LSM303A_INT2_CFG_A        0x34
#define LSM303A_INT2_SRC_A        0x35
#define LSM303A_INT2_THS_A        0x36
#define LSM303A_INT2_DURATION_A   0x37






// Constructors ////////////////////////////////////////////////////////////////

LSM303A::LSM303A(int file) : I2CDevice(file, LSM303A_ADDRESS)
{

	// Enable Accelerometer
	// 0x37 = 0b00110111
  // 400Hz
	// Normal power mode, all axes enabled
  writeByte(LSM303A_CTRL_REG1_A, 0x3F);
  writeByte(LSM303A_CTRL_REG1_A, 0x27);
	
}


// Reads the 3 accelerometer channels and stores them in vector a
void LSM303A::read()
{
  __u8 buf[6];
  readBlock(LSM303A_OUT_X_L_A | (1 << 7), buf, 6);


  for (int i = 0; i < 6; i++)
  {
    buf[i] = ~buf[i];
    buf[i] += 0x01;
  }
  /*
  printf("acc: ");
  for (int i = 0; i < 6; i++)
    printf("%02x ", buf[i]);
  printf("\n");
  */

  //__u8 test = 1; 
  //printf("test 1: %02X\n", test);
  //test = 240; 
  //printf("test 240: %02X\n", test);
	
  __u8 xla = buf[0];
  __u8 xha = buf[1];
  __u8 yla = buf[2];
  __u8 yha = buf[3];
  __u8 zla = buf[4];
  __u8 zha = buf[5];

  __s16 x = xha << 8 | xla;
  __s16 y = yha << 8 | yla;
  __s16 z = zha << 8 | zla;

	a.x() = x >> 4;
	a.y() = y >> 4;
	a.z() = z >> 4;
}


